#!/usr/bin/python
import pygame
import game_engine


class Tile:
    
    # 
    def __init__(self,x,y, endX, endY, parent, cost):
        self.parent = parent
        self.x = x
        self.y = y
        self.g = 0
        if parent != None:
            self.g = parent.getG() + cost
        self.h = (abs(endX-x) + abs(endY-y)) * cost

    # Set a new shortest-path parent for this node, including the cost to get to this tile from the parent
    def setParent(self, parent, cost):
        self.parent=parent
        self.g = parent.getG() + cost

    def getParent(self):
        return self.parent

    def getH(self):
        return self.h

    def getG(self):
        return self.g

    def getF(self):
        return self.h + self.g

    def getX(self):
        return self.x

    def getY(self):
        return self.y


class PathFinder:

    def __init__(self):
   
        self.m_level =  game_engine.GameEngine.getGameEngine().getLevelManager()
        self.m_tileSize = game_engine.GameEngine.getGameEngine().getLevelManager().getTileSize()
        self.m_levelSize = self.m_level.getLevelSize()
        
    # Return a path of tiles to traverse ordered in reverse order
    #    use path.pop() to repeatedly get the next step
    def generateReversePath(self, lastTile):
        path = []
        # Note: This chops off the final node, since that node is our starting position
        while (lastTile.getParent() != None):
            path.append( (lastTile.getX(), lastTile.getY()) )
            lastTile = lastTile.getParent()
        return path

    # Find a path to the specified endpoint. If it cannot be reached, fail
    def findPath(self, entity, (endX,endY)):

        # First check if the endpoint itself can be reached by this entity, if not fail early
        if not(self.m_level.isEntityWalkable(entity, endX, endY)) or not(self.m_level.getEntityCollisionMap().canEntityMoveTo(entity, (endX, endY))[0]):
            return [] # Unreachable

        # Call through to find path to the closest reachable spot
        solution = self.findPathToClosestReachable(entity, (endX, endY))

        # If this spot is not the desired spot, fail
        if len(solution) == 0 or solution[0][0] != endX or solution[0][1] != endY:
            return []

        # Otherwise, solution is good
        return solution


      
    # Find a path to the closest reachable spot to destination and return a list of tiles 
    def findPathToClosestReachable(self, entity, (endX,endY), ignoreEntities=[]):
        (startX, startY) = entity.getPosition()
        (width, height) = entity.getSize()
        # Track the set of squares that we've already evaluated best-cast scenarios for - the 'closed' list
        closedSet = dict()
        # Track the set of squares that are under consideration
        openSet = dict()

        
        # Add starting tile
        x = startX
        y = startY
        openSet[x + y * self.m_levelSize[0]] = Tile(x, y, endX, endY, None, 10)

        # Continue until we have a solution, or the open set is empty (no solution)
        examined = 0
        currentTile = None
        while (len(openSet) > 0):
            # Look for lowest cost square on open list
            currentTile = None
            for tile in openSet.values():
                # If there is no current tile, no need to compare
                if currentTile == None:
                    currentTile = tile
                    continue
                # Otherwise, find lowest cost tile on open list
                if tile.getF() < currentTile.getF():
                    currentTile = tile

            # Count one more node examined
            examined +=1

            # If the best pick is the end tile, we've found the shortest path
            x = currentTile.getX()
            y = currentTile.getY()
            if x == endX and y == endY:
                solution = self.generateReversePath(currentTile)
                #print "Pathing solution found with "+str(len(solution))+" steps. Took "+str(examined)+" cycles."
                return solution


            # Move this tile to the closed list
            del openSet[x + y * self.m_levelSize[0]]
            closedSet[x + y * self.m_levelSize[0]] = currentTile

            # Process the tiles around this that are directly reachable from this tile and add to openSet if needed
            # TODO: If we add diagonal movement, these can be collapsed into 2 nested loops from -1 to 1 on each axis
            for xmod in [-1,1]:
                if not((x+xmod + y * self.m_levelSize[0]) in closedSet):
                    if not((x+xmod + y * self.m_levelSize[0]) in openSet):
                        # Determine whether we can walk here
                        if not(self.m_level.isEntityWalkable(entity, x+xmod, y)) or not(self.m_level.getEntityCollisionMap().canEntityMoveTo(entity, (x+xmod, y), ignoreEntities)[0]):
                            # Add to closed set so we dont examine this again
                            closedSet[x+xmod + y * self.m_levelSize[0]] = Tile(x+xmod, y, endX, endY, None, 10)
                        else:
                            # Add to open set so we can examine
                            openSet[x+xmod + y * self.m_levelSize[0]] = Tile(x+xmod, y, endX, endY, currentTile, 10)
                    else:
                        openTile = openSet[x+xmod + y * self.m_levelSize[0]]
                        if currentTile.getG() + 10 < openTile.getG():
                            # Reparent tile to this one
                            openTile.setParent(currentTile, 10)
            for ymod in [-1,1]:
                if not((x + (y+ymod) * self.m_levelSize[0]) in closedSet):
                    if not((x + (y+ymod) * self.m_levelSize[0]) in openSet):
                        # Determine whether we can walk here
                        if not(self.m_level.isEntityWalkable(entity, x, y+ymod)) or not(self.m_level.getEntityCollisionMap().canEntityMoveTo(entity, (x, y+ymod), ignoreEntities)[0]):
                            # Add to closed set so we dont examine this again
                            closedSet[x+ (y+ymod) * self.m_levelSize[0]] = Tile(x, y+ymod, endX, endY, None, 10)
                        else:
                            # Add to open set so we can examine
                            openSet[x+ (y+ymod) * self.m_levelSize[0]] = Tile(x, y+ymod, endX, endY, currentTile, 10)
                    else:
                        openTile = openSet[x + (y+ymod) * self.m_levelSize[0]]
                        if currentTile.getG() + 10 < openTile.getG():
                            # Reparent tile to this one
                            openTile.setParent(currentTile, 10)

        # No solution, search for lowest H in closed set
        for tile in closedSet.values():
            if tile.getParent() is None:
                continue
            if tile.getH() < currentTile.getH():
                currentTile = tile

        # Generate a solution to this tile
        solution = self.generateReversePath(currentTile)
        #print "No exact path found in "+str(examined)+" cycles. Took "+str(len(solution))+" steps and got to "+str(currentTile.getX())+","+str(currentTile.getY())
        return solution



